/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package dca.sistemascontrole.malhadecontrole;

import dca.sistemascontrole.domain.VariaveisDeControle;
import dca.sistemascontrole.travas.GerenciadorDeTravas;
import dca.sistemascontrole.util.ControleConstantes;

/**
 *
 * @author Rafael
 */
public class ControladorPI_D extends MalhaFechada {

    private double up, ui, ud;
    private double integralPID;
    private double nivelAnteriorPI_D;
    private double udAnteriorPID;
    private double uiAnterior;

    public ControladorPI_D(double setpoint, double kp, double ki, double kd, boolean usarFiltroDerivativo, boolean usarFiltroAntiWindup) {
        super(setpoint);
        this.kp = kp;
        this.kd = kd;
        this.ki = ki;
        this.usarFiltroDerivativo = usarFiltroDerivativo;
        this.usarFiltroAntiWindup = usarFiltroAntiWindup;
        integralPID = 0;
        nivelAnteriorPI_D = Double.MIN_VALUE;
        udAnteriorPID = Double.MIN_VALUE;
        uiAnterior = 0;
    }

    @Override
    public int getTipo() {
        return PI_D;
    }

    @Override
    protected VariaveisDeControle getNextValue(long t, double nivelTanqueSuperior, double nivelTanqueInfeior) {
        double erro;
        double U;
        double Usat;
        double pv;

        if (ControleConstantes.PV_IS_TANQUE_SUPERIOR) {
            pv = nivelTanqueSuperior;
        } else {
            pv = nivelTanqueInfeior;
        }
        erro = variaveisDeControle.getSetPoint() - nivelTanqueSuperior;
        integralPID = integralPID + ki * erro * 0.1;
        if (nivelAnteriorPI_D != Double.MIN_VALUE) {
            if (usarFiltroDerivativo) {
                //ud = (kd * (nivelTanqueSuperior - nivelAnteriorPI_D))/((kd*gama*(nivelTanqueSuperior - nivelAnteriorPI_D))+1);
                ud = ((kd * (pv - nivelAnteriorPI_D)) + (udAnteriorPID * kd * gama)) / (0.1 + kd * gama);
            } else {
                ud = kd * (pv - nivelAnteriorPI_D);
            }
        } else {
            ud = 0;
        }

        up = kp * erro;

        if (usarFiltroAntiWindup) {
            integralPID = integralPID + (uiAnterior * kp) / tw;
        }
        ui = integralPID;
        U = up + ui + ud;
        Usat = GerenciadorDeTravas.ajustarAsTravas(pv, U);
        uiAnterior = (Usat - U);

//        ui = integralPID;
//        if (usarFiltroAntiWindup) {
//            U = up + ui + ud;
//            Usat = GerenciadorDeTravas.ajustarAsTravas(nivelTanqueSuperior, U);
//            integralPID = integralPID + ((Usat-U)*kp)/tauAk;
//            U = U + ((3-U)*kp)/tauAk;
//        }else{
//        U = up + ui + ud;
//        }

        nivelAnteriorPI_D = pv;

        variaveisDeControle.setU(U);
        variaveisDeControle.setUd(ud);
        variaveisDeControle.setUi(ui);
        variaveisDeControle.setUp(up);
        variaveisDeControle.setErro(erro);

        return variaveisDeControle;
    }
}
